#include "movejoint.h"
#include<QThread>

MoveJoint::MoveJoint(QWidget *parent) : QWidget(parent)
{
    // //mPublish = std::make_unique<CNPClient>(0, "ORS_CMOR");
    // CNP_DOMAIN_ID domain = 0;
    // std::string serverAppID = "ORS_CMOR";
    // mPublish = new CNPClient(domain, serverAppID);
    // pubSwitchStatu();
    // std::cout<<"in MoveJoin Contrut "<<std::endl;
}

MoveJoint::MoveJoint(const QJsonObject &jointDetails, QWidget *parent)
    : QWidget(parent){

    // CNP_DOMAIN_ID domain = 0;
    // std::string serverAppID = "ORS_CMOR";
    // mPublish = new CNPClient(domain, serverAppID);
    // {
    //     QMutexLocker locker(&mutex_);
    //     subStatus();
    // }
    // pubSwitchStatu();
    // //std::cout<<"in MoveJoin Contrut "<<std::endl;

}

MoveJoint::MoveJoint(const QString &Name, const QJsonObject &functionDetails, QWidget *parent)
    : QWidget(parent){
    QGroupBox *groupBox = new QGroupBox(Name, this);
    onceFlag = 0;
    CNP_DOMAIN_ID domain = 0;
    std::string serverAppID = "ORS_CMOR";
    mPublish = new CNPClient(domain, serverAppID);
    //发布切换Enable状态
    subStatus();
    pubSwitchStatu();
    QThread::msleep(500);
    groupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
     // 调用 createMoveJoint 进行初始化
    createMoveJoint(groupBox, functionDetails);

    // 创建 QVBoxLayout 并设置到当前 QWidget
    QVBoxLayout *layout = new QVBoxLayout(this);
    layout->addWidget(groupBox); // 将 groupBox 添加到布局
    layout->setContentsMargins(0, 0, 0, 0); // 可选，去除边距
    // 让 MoveJoint 使用这个布局
    setLayout(layout);
}
/**********************************************
* @file       mainwindow.cpp
* @brief      控制关节滑动函数代码实现
* 首先设置了一个布局，然后循环创建每一个joint
* @author     yfk
* @date       2025-02-11
**********************************************/
void MoveJoint::createMoveJoint(QGroupBox *groupBox, const QJsonObject &functionDetails)
{
    // 设置 groupBox 的主布局为水平布局（QHBoxLayout）
    QHBoxLayout *mainLayout = new QHBoxLayout();
    groupBox->setLayout(mainLayout);

    // 创建用于放置关节控件的窗口
    QWidget *jointsWidget = new QWidget(groupBox);
    QVBoxLayout *jointsLayout = new QVBoxLayout(jointsWidget);

    jointsWidget->setLayout(jointsLayout);
    jointsWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);

    // 设置垂直布局的间距为 10 像素
    jointsLayout->setSpacing(5);  // 设置每个 Joint 控件之间的间距为 10 像素
    jointsLayout->setContentsMargins(0, 0, 0, 0);

    jointsLayoutRef = jointsLayout;  // 保存指针，后面可以用
    // 创建用于放置复位按钮的窗口
    QWidget *homeButtonWidget = new QWidget(groupBox);
    QVBoxLayout *homeButtonLayout = new QVBoxLayout(homeButtonWidget);
    homeButtonWidget->setLayout(homeButtonLayout);
    homeButtonWidget->setMaximumWidth(100);  // 将按钮窗口的宽度最大设为 200 像素

    // 创建 "Home" 按钮
    QPushButton *homeButton = new QPushButton("Home", homeButtonWidget);
    // 设置按钮扩展以充满 homeButtonWidget
    homeButton->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
    homeButtonLayout->addWidget(homeButton);

    //    // 点击 "Home" 按钮时，将所有关节复位
    connect(homeButton, &QPushButton::clicked, this, [this, jointsLayout]() {
       // 遍历关节控件并调用 reset 方法归位
        for (int i = 0; i < jointsLayout->count(); ++i) {
            QWidget *widget = jointsLayout->itemAt(i)->widget();
            if (widget && widget->inherits("Joint")) {
                Joint *joint = qobject_cast<Joint *>(widget);
                if (joint) {
                    QString name = joint->getJointName();
                    // QString angle = joint->getAngleDisplay()->text();
                    joint->reset(name);  // 调用 Joint 类中的 reset 方法
                }


            }
        }

        //发布消息
            InitPose startHomePose = getInitPose();//获取关节初始位置
        if(startHomePose.jointAngle.size()>1)
          {  QString strJointPose=QString::number(startHomePose.jointAngle[0]);
            // // 组装为字符串格式
            for(int i=1;i<startHomePose.jointAngle.size();i++)
            {
                strJointPose=strJointPose+","+QString::number(startHomePose.jointAngle[i]);
            }
            QString qcontent = "[" + strJointPose + "]";
            QString pubcontent = "moveJoint(q="+qcontent+",a=1,v=1)";

            msgMoveJoint.device_id("ORS_CMOR");
            auto now = std::chrono::system_clock::now();
            auto timestamp_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                                    now.time_since_epoch()
                                    ).count();
            msgMoveJoint.device_id(std::to_string(timestamp_ms));
            msgMoveJoint.content(pubcontent.toStdString());
            if (!mPublish)
                std::cout << "mPbush is dead" << std::endl;
            else
            {
                CNP_RESULT result = mPublish->publishCmdRequest(msgMoveJoint);
                if (result == CNP_RESULT::CNP_PUBLISH_SUCCESS) {
                    std::cout << "Message " << msgMoveJoint.content() << " published successfully." << std::endl;
                }
                else {
                    std::cout << "Failed to publish message " << "." << std::endl;
                }
            }
        }

       //  pubMoveJoint("joint_1",0);
       //  //QThread::msleep(5000);
       //  pubMoveJoint("joint_2",0);
       //  //::msleep(5000);
       //  pubMoveJoint("joint_3",0);
       // // QThread::msleep(5000);
       //  pubMoveJoint("joint_4",0);
       // // QThread::msleep(5000);
       //  pubMoveJoint("joint_5",0);
       // // QThread::msleep(5000);
       //  pubMoveJoint("joint_6",0);
       // //QThread::msleep(5000);
       //  pubMoveJoint("joint_7",0);


    });

    InitPose startPose = getInitPose();//获取关节初始位置
    JointDefault startname;
    for(int i=0;i<startPose.jointAngle.size();i++)
    {
        startname.jointName="joint_"+QString::number(i+1);
        startname.defaultValue=startPose.jointAngle[i];
        jointDefaults.replace(i,startname);
        //std::cout<<"startname.jointName="<<startname.jointName<<"startname.defaultValue="<<startname.defaultValue<<std::endl;
    }
    // 遍历 functionDetails 中的所有关节并为每个关节创建控件
    for (auto it = functionDetails.begin(); it != functionDetails.end(); ++it) {
        QString jointName = it.key();
        QJsonObject jointDetails = it.value().toObject();
        qDebug() << "JointName: " << jointName;
        qDebug() << "Joint Details: " << jointDetails;
        // qDebug() << "测试是否进入movejoint ";

        if (jointName == "uiElement")
            continue;
        // 在 jointDefaults 中查找与当前关节名称匹配的默认值
        for (const JointDefault &jointDefault : jointDefaults) {
            qDebug() << "Checking jointName:" << jointName << "against jointDefault.jointName:" << jointDefault.jointName;

            if (jointName == jointDefault.jointName) {
                // 找到匹配的关节名称，更新默认值
                qDebug() << "Match found! Updating DefaultValue for" << jointName << "to" << jointDefault.defaultValue;
                jointDetails["DefaultValue"] = jointDefault.defaultValue;
                break;  // 退出内层循环
            }
        }

        // 输出修改后的 jointDetails，查看是否成功更新
        qDebug() << "Updated Joint Details for" << jointName << ":" << jointDetails;

        // 创建 Joint 控件并添加到关节控件窗口
        Joint *jointControl = new Joint(jointName, jointDetails);
        connect(jointControl, &Joint::angleChanged, this, &MoveJoint::onJointAngleChanged);
        jointsLayout->addWidget(jointControl);
    }
    getJointAngles(jointsLayout);
    // 将关节窗口和复位按钮窗口添加到主布局
    mainLayout->addWidget(jointsWidget);        // 放置所有关节控件的窗口
    mainLayout->addWidget(homeButtonWidget);    // 放置复位按钮的窗口
    //mainLayout->setSpacing(100);  // 设置两者之间的间距为 20 像素
}

void MoveJoint::getJointAngles(QVBoxLayout *jointsLayout)
{
    // 遍历所有关节控件，获取它们的角度值
    for (int i = 0; i < jointsLayout->count(); ++i) {
        QWidget *widget = jointsLayout->itemAt(i)->widget();
        if (widget && widget->inherits("Joint")) {
            Joint *joint = qobject_cast<Joint *>(widget);
            if (joint) {
                QString name = joint->getJointName();
                QString angle = joint->getAngleDisplay()->text();
                jointAngles.push_back({ name, angle });
                qDebug() << "Joint: " << name << " Angle: " << angle;
            }
        }
    }
}


void MoveJoint::onJointAngleChanged(const QString &jointName, double angle)
{
    qDebug() << "Joint " << jointName << " angle changed to: " << angle;

    // 实时更新 jointAngles vector
    bool found = false;
    for (auto &pair : jointAngles) {
        if (pair.first == jointName) {
            pair.second = QString::number(angle);
            found = true;
            break;
        }
    }
    pubMoveJoint(jointName,angle);//发布MoveJoint界面操作内容
    // 如果没找到该 jointName，说明是新加的 joint
    if (!found) {
        jointAngles.push_back({ jointName, QString::number(angle) });
    }
}

void MoveJoint::pubSwitchStatu()
{
    CnpMsg msgSwitchStatu;
    msgSwitchStatu.device_id("ORS_CMOR");
     auto now = std::chrono::system_clock::now();
    auto timestamp_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                            now.time_since_epoch()
                            ).count();
     msgSwitchStatu.device_id(std::to_string(timestamp_ms));

     QString pubcontent = "switchState(s=Enable)";
     msgSwitchStatu.content(pubcontent.toStdString());
     if (!mPublish)
         std::cout << "mPbush is dead" << std::endl;
     else
     {
         CNP_RESULT result = mPublish->publishCmdRequest(msgSwitchStatu);
         if (result == CNP_RESULT::CNP_PUBLISH_SUCCESS) {
             std::cout << "Message " << msgSwitchStatu.content() << " published successfully." << std::endl;
         }
         else {
             std::cout << "Failed to publish message " << "." << std::endl;
         }
     }
}

void MoveJoint::pubMoveJoint(const QString &jointName, double angle)
{
    // QStringList list;
    // // 依次取出每个 pair 的 second 值
    // for (const auto &pair : jointAngles) {
    //     list << pair.second;
    // }
    // // 组装为字符串格式
    // QString qcontent = "[" + list.join(",") + "]";
    //QString pubcontent = "moveJoint(q="+qcontent+",a=1,v=1)";

    QString jointNumber = jointName.right(1);
    QString pubcontent = "moveSingleJoint(i="+jointNumber+",q="+QString::number(angle)+")";//moveSingleJoint(i=1, q=80)


    //CnpMsg msgMoveJoint;
    msgMoveJoint.device_id("ORS_CMOR");
    auto now = std::chrono::system_clock::now();
    auto timestamp_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                            now.time_since_epoch()
                            ).count();
    msgMoveJoint.device_id(std::to_string(timestamp_ms));
    msgMoveJoint.content(pubcontent.toStdString());
    if (!mPublish)
        std::cout << "mPbush is dead" << std::endl;
    else
    {
        CNP_RESULT result = mPublish->publishCmdRequest(msgMoveJoint);
        if (result == CNP_RESULT::CNP_PUBLISH_SUCCESS) {
            std::cout << "Message " << msgMoveJoint.content() << " published successfully." << std::endl;
        }
        else {
            std::cout << "Failed to publish message " << "." << std::endl;
        }
    }

}

void MoveJoint::subStatus()
{
    if (!mPublish) {
        qWarning() << "mPublish subStatuScriber not initialized";
        //return CNP_RESULT::CNP_ERROR;
    }

    // 使用 lambda 将 subscribeCallback 转为成员函数调用
    auto callback = [this](const CnpMsg &msg) {
        this->initPoseCallBack(msg);
    };

    // 这里直接传入 lambda，假设 subscribeCmdRequest 接收 std::function
    CNP_RESULT result = mPublish->subscribeStatus(callback);

    if (result == CNP_RESULT::CNP_SUBSCRIBE_SUCCESS)
    {
        qDebug() << "mPublish Subscription started successfully.";
    }
    else
    {
        qDebug() << "mPublish Subscription failed.";
    }

}



void MoveJoint::initPoseCallBack(const CnpMsg &msg) {
    // 先将 msg.content() 存储到局部变量，再捕获
    if(onceFlag == 0)
    {
        std::string content = msg.content();

        processStatuMessage(QString::fromStdString(content));

        onceFlag = 1;

    }
}

void MoveJoint::processStatuMessage(const QString &msg)
{
    QMutexLocker locker(&mutex_);
    mInitPose.endPose.clear();
    mInitPose.jointAngle.clear();
    mInitPose.jointVelocity.clear();
    std::cout<<"in processStatuMessage !!!"<<std::endl;
    QJsonParseError parseError;
    QJsonDocument doc = QJsonDocument::fromJson(msg.toUtf8(), &parseError);

    if (parseError.error != QJsonParseError::NoError) {
        qDebug() << "JSON 解析错误:" << parseError.errorString();
        return;
    }

    QJsonObject root = doc.object();

    // 获取 device_id 字段
    QString deviceIdStr = root.value("device_id").toString();

    // 获取 message_id 字段作为 timestamp
    QString timestampStr = root.value("message_id").toString();

    // 解析 content 对象，存入 QHash<QString, std::vector<double>>
    // 提取 endPose 数组

    if (root.contains("endPose") && root["endPose"].isArray()) {
        QJsonArray jsonArray = root["endPose"].toArray();
        std::vector<double> endPoseVec;
        for (const auto &value : jsonArray) {
            endPoseVec.push_back(value.toDouble());
            mInitPose.endPose.push_back(value.toDouble());
        }
        // for(int i=0;i<6;i++)
        // {
        //     mInitPose.endPose[i]= endPoseVec[i];
        // }

    }

    // 提取 jointAngle 数组

    if (root.contains("jointAngle") && root["jointAngle"].isArray()) {
        QJsonArray jsonArray = root["jointAngle"].toArray();
        std::vector<double> jointAngleVec;
        for (const auto &value : jsonArray) {
            jointAngleVec.push_back(value.toDouble());
            mInitPose.jointAngle.push_back(value.toDouble());
        }
        // for(int i=0;i<7;i++)
        // {
        //     mInitPose.jointAngle[i]= jointAngleVec[i];
        // }

    }

    // 提取 jointVelocity 数组

    if (root.contains("jointVelocity") && root["jointVelocity"].isArray()) {
        QJsonArray jsonArray = root["jointVelocity"].toArray();
        std::vector<double> jointVelocityVec;
        for (const auto &value : jsonArray) {
            jointVelocityVec.push_back(value.toDouble());
            mInitPose.jointVelocity.push_back(value.toDouble());
        }
        // for(int i=0;i<7;i++)
        // {
        //     mInitPose.jointVelocity[i]= jointVelocityVec[i];
        // }
    }
}
